#include "DynamicTrimesh.h"

DynamicTrimesh::DynamicTrimesh(dWorldID &world, dSpaceID &space, std::vector<Vertex> &verts, std::vector<GLuint> &indices) : Object(verts, indices)
{
	data = dGeomTriMeshDataCreate();

	dGeomTriMeshDataBuildSingle(data, (dReal*)&DynamicTrimesh::vertices[0].position[0], sizeof(Vertex), DynamicTrimesh::vertices.size(), (dTriIndex*)&DynamicTrimesh::indices[0], DynamicTrimesh::indices.size(), sizeof(GLuint) * 3);

	// TODO: FIX THIS. Needs correct mass
	body = dBodyCreate(world);
	geom = dCreateTriMesh(space, data, 0, 0, 0);

	dMass m;
	dMassSetBox(&m, 1.0f, 1, 1, 1);   	
	dMassAdjust(&m, 3);
	dBodySetMass(body, &m);

	dGeomSetBody(geom, body); 
}

DynamicTrimesh::DynamicTrimesh(dWorldID &world, dSpaceID &space, char *fileName, bool normaliseModel)
{
	int i;
	ModelOBJ model;

	data = dGeomTriMeshDataCreate();

	// Import the OBJ file and normalize to unit length.
    if (!model.import(fileName))
    {
        throw std::runtime_error("Failed to load model.");
    }

	if(normaliseModel)
		model.normalize();

	for(i = 0; i < model.getNumberOfVertices(); ++i)
	{
		Vertex currentVertex;
		currentVertex.position = (cml::vector3f(model.getVertex(i).position[0], model.getVertex(i).position[1], model.getVertex(i).position[2]));
		currentVertex.normal = (cml::vector3f(model.getVertex(i).normal[0], model.getVertex(i).normal[1], model.getVertex(i).normal[2]));

		vertices.push_back(currentVertex);

	}

	for(i = 0; i < model.getNumberOfIndices(); ++i)
	{
		indices.push_back(model.getIndexBuffer()[i]);
	}

	dGeomTriMeshDataBuildSingle(data, (dReal*)&DynamicTrimesh::vertices[0].position[0], sizeof(Vertex), DynamicTrimesh::vertices.size(), (dTriIndex*)&DynamicTrimesh::indices[0], DynamicTrimesh::indices.size(), sizeof(GLuint) * 3);

	// TODO: FIX THIS. Needs correct mass
	body = dBodyCreate(world);
	geom = dCreateTriMesh(space, data, 0, 0, 0);

	dMass m;
	dMassSetBox(&m, 1.0f, 1, 1, 1);   	
	dMassAdjust(&m, 3);
	dBodySetMass(body, &m);

	dGeomSetBody(geom, body); 
}

void DynamicTrimesh::renderInLocalFrame() const
{
	glColor3ubv(color);

	GLfloat mat_specular[] = {0.1f, 0.1f, 0.1f};
	glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);

	glEnableClientState(GL_VERTEX_ARRAY);
	glEnableClientState(GL_NORMAL_ARRAY);

	glVertexPointer(3, GL_FLOAT, sizeof(Vertex), &vertices[0].position);
	glNormalPointer(GL_FLOAT, sizeof(Vertex), &vertices[0].normal);

	glDrawElements(GL_TRIANGLES, indices.size(), GL_UNSIGNED_INT, &indices[0]);

	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_NORMAL_ARRAY);
}